#include "baseOrbit.hpp"
#include "starSystem.hpp"

baseOrbit::baseOrbit()
{
	renderBody.setFillColor(screen::PLANETCOL);
}


void baseOrbit::tick(int64_t timeStep)
{
	basePosition::tick(timeStep);
	setState( stateFromOrbit( getOrbit() , phys::globTime ) );

	if( getOrbit().shape() != move_t::FIXED )
	{
		auto parent = getOrbit().par.lock();
		auto system = getSys().lock();

		if( getState().pabs() > parent->getSOI() )
		{
			system->kickList.push_back(self.lock());
		}
	}

}


void baseOrbit::refreshDisp()
{
	basePosition::refreshDisp();
	if( !getParent().expired() )
	{
		stvec st = getParent().lock()->getState();
		renderOrbit.setPosition(st.px, st.py);
	}
}

/*
void baseOrbit::calcPath()
{
	if(moveType == move_t::ORBIT_E && !getParent().expired())
	{
		// calculate full elliptical orbit
		double M;
		double E;
		double E1;
		double arg;
		double rad;
		double periaps;
		double T;
		int64_t t;
		int i;
		int N = screen::ORBITPCOUNT;
		sf::Vector2f tmpVec;
		sf::Vertex   tmpVert;
		std::vector<sf::Vertex> vertexList;
		auto parent = getParent().lock();

		renderOrbit.clear();

		// periapsis calculation
		periaps = getSMA() * (1 - getEcc()*getEcc() ) / ( 1 + getEcc() );

		// time where E = pi/2
		T = (phys::PI/2-getEcc())/getN();

		for( i = 0; i < N/2; i++ )
		{
			t = (int64_t) i*2*(pPeriod-2*T)/N + T;
			M = getN()*t;
			E = M;
			do
			{
				E1 = E;
				E = M + getEcc() * sin(E1);
			} while( fabs(E - E1) > phys::ERR );

			arg = 2*atan(sqrt( ( 1 + getEcc() ) / ( 1 - getEcc() ) ) * tan( E/2 )); // note lack of getTheta(), requires rotation after construction
			rad = getSMA() * (1 - getEcc()*getEcc() ) / ( 1 + getEcc() * cos(arg) );

			tmpVec.x = rad * cos(arg); // automatically centers on parent
			tmpVec.y = rad * sin(arg);
			tmpVert.position = tmpVec;
			vertexList.push_back(tmpVert);
		}

		for( i = 0; i < N/2; i++)
		{
			renderOrbit.append(vertexList[i]);
			tmpVec.x = -2*(getSMA()-periaps)-vertexList[i].position.x;
			tmpVec.y = -vertexList[i].position.y;
			tmpVert.position = tmpVec;
			vertexList[i] = tmpVert;
		}

		for( i = 0; i < N/2; i++ )
		{
			renderOrbit.append(vertexList[i]);
		}

		renderOrbit.append(renderOrbit[0]); // completes the circle, ideally

		renderOrbit.setRotation(radToDeg(getTheta()));
		renderOrbit.setPosition(parent->getPosX(), parent->getPosY());

	}
	else if(moveType == move_t::ORBIT_H && !getParent().expired())
	{
		renderOrbit.clear();
		std::shared_ptr<basePosition> parent = getParent().lock();
		double x;
		double y;
		double ymax = parent->getSOI()/(-getEcc()*getSMA());
		double ymin = 1e5/sqrt(( -parent->getMu()/getSMA() )*( 1+getEcc() )/( getEcc()-1 ));
		double k; // scaling factor for y
		double up;     // bounds for binary search
		double down;
		double trial;
		int N = screen::ORBITPCOUNT/2;
		int i;
		sf::Vector2f tmpVec;
		sf::Vertex   tmpVert;
		std::vector<sf::Vertex> vertexList;

		// finds "appropriate" scaling constant
		up   = 100;
		down = 0.99;
		do
		{
			k = 0.5*(up+down);
			trial = pow(k,N+1) - 1 - (ymax/ymin)*(k-1);
			if(trial > 0)
			{
				up = k;
			}
			else
			{
				down = k;
			}
		} while( fabs(up - down) > phys::ERR );

		for( i = 0; i < N; i++)
		{
			y = pow(k,i)*ymin;
			x = -sqrt( (y*y)/(getEcc()*getEcc()-1) + getSMA()*getSMA() ) - getSMA()*getEcc();
			tmpVec.x = x;
			tmpVec.y = y;
			tmpVert.position = tmpVec;
			vertexList.push_back(tmpVert);
		}

		for( i = 1; i <= N; i++)
		{
			renderOrbit.append(vertexList[N-i]);
			tmpVec.x =  vertexList[N-i].position.x;
			tmpVec.y = -vertexList[N-i].position.y;
			tmpVert.position = tmpVec;
			vertexList[N-i] = tmpVert;
		}

		for( i = 0; i < N; i++ )
		{
			renderOrbit.append(vertexList[i]);
		}

		renderOrbit.setRotation(-radToDeg(getTheta()));
		renderOrbit.setPosition(parent->getPosX(), parent->getPosY());
	}
	else if( !getParent().expired() )
	{
		// manually calculate next (some time) of path, could adjust point spacing depending on velocity

		stvec        tmpState;
		sf::Vector2f tmpVec;
		sf::Vertex   tmpVert;

		renderOrbit.clear();

		for( int i = -200; i < 1000; i++ )
		{
			tmpState = getTStateAbs(phys::globTime + i * 86400);
			tmpVec.x = tmpState.px;
			tmpVec.y = tmpState.py;
			tmpVert.position = tmpVec;
			renderOrbit.append(tmpVert);
		}
	}
}
*/
/*
void baseOrbit::orbitFromState()
{
	if( !getParent().expired() )
	{
		auto par = getParent().lock();
		stvec st = getState();

		double r = st.pabs();
		double v = st.vabs();
		double l = st.px*st.vy - st.py*st.vx;
		double mu= par->getMu();
		int d = sgn(l);

		// eccentricity vector & eccentricity
		double ex  = (v*v*st.px - (st.px*st.vx + st.py*st.vy)*st.vx)/mu - st.px/r;
		double ey  = (v*v*st.py - (st.px*st.vx + st.py*st.vy)*st.vy)/mu - st.py/r;
		double ecc = sqrt(ex*ex+ey*ey);
		setEcc(ecc);

		// specific energy
		double E =  (v*v/2)-mu/r;

		// semi-major axis, mean motion
		double sma = -mu/(2*E);
		setSMA(sma);

		double n;
		double nu;
		double theta;
		int64_t T;

		if(ecc < 1)
		{
			// elliptical orbit
			std::cout << "elliptical orbit\n";
			n   = sqrt(mu/(sma*sma*sma));

			nu = acos((ex*st.px + ey*st.py)/(ecc*r)); // true anomaly ie angle from periaps
			if((st.vx*st.px+st.vy*st.vy) < 0)
				nu = 2*phys::PI-nu;

			theta = nu - acos(st.px/r); // argument of periaps
//			theta = nu - atan2(st.px,st.py); // argument of periaps

			double EA = d*2*atan(sqrt((1-ecc)/(1+ecc)) * tan(nu/2));
			T = (EA-ecc*sin(EA))/n;

			setT0(T);
			setTheta(theta);
			setDirec(d);
			setMove(move_t::ORBIT_E);
		}
		else
		{
			// hyperbolic orbit
			std::cout << "hyperbolic orbit\n";
			n   = sqrt(-mu/(sma*sma*sma));

			nu = acos((ex*st.px + ey*st.py)/(ecc*r)); // true anomaly ie angle from periaps
			if(st.vx*st.px+st.vy*st.vy < 0)
				nu = 2*phys::PI-nu;

			theta = nu - acos(st.px/r); // argument of periaps
//			theta = (nu - atan2(st.px, st.py)); // argument of periaps
			double HA = d*2*atanh(sqrt((ecc-1)/(ecc+1)) * tan(nu/2));
			T = (ecc*sinh(HA)-HA)/n;

			setT0(T);
			setTheta(theta);
			setDirec(d);
			setMove(move_t::ORBIT_H);
		}

		std::cout << "direc: " << d     << "\n";
		std::cout << "theta: " << theta << "\n";
		std::cout << "SMA  : " << sma   << "\n";
		std::cout << "Ecc  : " << ecc   << "\n";
		std::cout << "T0   : " << T     << "\n";
		std::cout << "n    : " << n     << "\n";
	}
}
*/
/*
void baseOrbit::orbitFromState()
{
	if( !getParent().expired() )
	{
		auto par = getParent().lock();
		stvec st = getState();

		double arg = atan2(st.px, st.py);

		double r = st.pabs();
		double v = st.vabs();
		double h = st.px*st.vy - st.py*st.vx;
		double mu= par->getMu();
		int d = sgn(h);

		// eccentricity vector & eccentricity
		double ex  = ( st.vy*h)/mu - st.px/r;
		double ey  = (-st.vx*h)/mu - st.py/r;
		double ecc = sqrt(ex*ex+ey*ey);
		setEcc(ecc);

		// true anomaly
		double nu = acos((ex*st.px+ey*st.py)/(ecc*r));
		if((st.px*st.vx+st.py*st.vy) < 0)
			nu = 2*phys::PI-nu;

		// semi-major axis
		double sma = 1/((2/r)-(v*v/mu));
		setSMA(sma);

		// argument of periaps
		double theta = atan2(ey,ex);
//		double theta = nu - arg;
		int64_t T;

		if(ecc < 1)
		{
			// elliptical orbit
			std::cout << "elliptical orbit\n";

			// eccentric anomaly
			double EA = 2*atan(sqrt((1-ecc)/(1+ecc)) * tan(nu/2));

			// time of periapsis passage
			T = d*(EA-ecc*sin(EA))/getN();


			setMove(move_t::ORBIT_E);
		}
		else
		{
			// hyperbolic orbit
			std::cout << "hyperbolic orbit\n";

			// hyperbolic anomaly
			double HA = 2*atanh(sqrt((ecc-1)/(ecc+1)) * tan(nu/2));

			// time of periapsis passage
			T = phys::globTime - d*(ecc*sinh(HA)-HA)/getN();

			setMove(move_t::ORBIT_H);
		}

		setT0(T);
		setTheta(theta);
		setDirec(d);

		std::cout << "direc: " << d     << "\n";
		std::cout << "theta: " << theta << "\n";
		std::cout << "SMA  : " << sma   << "\n";
		std::cout << "Ecc  : " << ecc   << "\n";
		std::cout << "T0   : " << T     << "\n";
	}
}
*/
void baseOrbit::draw(sf::RenderWindow * window)
{
	window->draw(renderOrbit);
	basePosition::draw(window);
}


stvec baseOrbit::getTState(int64_t t)
{
	stvec outstate = stateFromOrbit( getOrbit(), t );

	return(outstate);
}


void baseOrbit::setMu(double inmu)
{
	basePosition::setMu(inmu);

	orvec orb = getOrbit();

	/* calculates min. altitude of L1 point r = R cubert( parent->mass/ 3*mass ), R = SMA*(1-ECC) */
	if(!orb.par.expired())
	{
		auto par = orb.par.lock();
		setSOI(orb.sma * (1-orb.ecc) * std::cbrt( getMu() / (3*par->getMu()) ) );
	}

}


char baseOrbit::getMove()
{
	return(getOrbit().shape());
}


void baseOrbit::setOrbit(orvec inorb)
{
	orbit = inorb;
	setParentw(orbit.par);
}
orvec baseOrbit::getOrbit()
{
	return(orbit);
}

void baseOrbit::setState(stvec instate)
{
	basePosition::setState(instate);
}

void baseOrbit::updateOrbit()
{
	setOrbit( orbitFromState( getState() ) );
}





void baseOrbit::setParentw(std::weak_ptr<basePosition> inparent)
{
	basePosition::setParentw(inparent);
	orbit.par = inparent;
	orvec orb = getOrbit();

	/* calculates min. altitude of L1 point r = R cubert( parent->mass/ 3*mass ), R = SMA*(1-ECC)*/
	auto par = orb.par.lock();
	setSOI(orb.sma * (1-orb.ecc) * std::cbrt( getMu() / (3*par->getMu()) ) );

	pathFromOrbit( &renderOrbit, getOrbit() );
}
void baseOrbit::setParents(std::shared_ptr<basePosition> inparent)
{
	baseOrbit::setParentw(std::weak_ptr<basePosition>(inparent));
}
